/**
 * @file low-level.cpp
 * @author Julien Mathieu
 * hier komt de echte code van low-level.h
 */

#include "sendcmd.h"

sendcmd::sendcmd(RobotData *rob, Client *client) {
	printf("sendcmd gecreeerd!!!!\n");
	call = new LowLevel();
	printf("na lowlevel\n");
	robot=rob;
	robotd_x=robot->get_robotX();
	robotd_y=robot->get_robotY();
	robotd_angle=robot->get_angle();
	printf("voor cmdgenerator\n");
	cmdgen=new CmdGenerator();
	printf("na cmdgenerator\n");
	WifiClient = client;
	//Constructor code here
	printf("sendcmd finished!!!!!!!!!!\n");
}

sendcmd::~sendcmd() {
	//Destructor code here
}

//roept simon zijn code op om het robotje te doen rijden
//krijgt een hoek tegenover vorige stand binnen waarover dat em moet draaien (komt binnen tussen 
//-180°en 180° binnen)
//en krijgt ook een afstand in pixels binnen waarover hij moet rijden
void sendcmd::drive(int afstand,float hoek)
{
	printf("start berekening van commandos\n");
	int absafstand,tijd;
	robotd_angle=robot->get_angle();
	robotd_angle+=hoek;
	if(hoek!=0)//robotje moet eerst draaien
	{
		//move(-100,100) voor 5 sec ==> +90°
		//move(100,-100) voor 5 sec ==> -90°

		tijd=abs(hoek)/HOEKPERSEC;
		if(hoek>0)
			stuurcomd(tijd,1);	//rechts draaien
		else
			stuurcomd(tijd,0);	//links draaien
	}
	
	//uiteindelijk positie berekenen
	robotd_x+=cos(robotd_angle)*afstand;
	robotd_y+=sin(robotd_angle)*afstand;


	absafstand=abs(afstand);	//absolute afstand
	tijd=afstand/PIXPSEC;
	if(afstand>0)
		stuurcomd(tijd,2);	//vooruit rijden
	else
		stuurcomd(tijd,3); 	//achteruit 
	
	//controleert of de positie is bereikt die bereikt moet worden
	//controleerpositie();

	printf("einde berekening, robot moet iets gedaan hebben\n");
}
void sendcmd::stuurcomd(int tijd,int geval)
{
	int warning;
	switch (geval)
	{
		//0 is links draaien
		case 0:	
			warning=call->move(-LSPEED,RSPEED);
			warning=call->move(-LSPEED,RSPEED);
			printf("rotate left for %d seconds\n", tijd);
			sleep(tijd);
			//controlleerdraai(tijd);
			break;
		//1 is rechts draaien
		case 1:
			warning=call->move(LSPEED,-RSPEED);
			warning=call->move(LSPEED,-RSPEED);
			printf("rotate right for %d seconds\n", tijd);
			sleep(tijd);
			//controlleerdraai(tijd);
			break;
		//2 rechtdoor rijden
		case 2:
			warning=call->move(LSPEED,RSPEED);
			warning=call->move(LSPEED,RSPEED);
			printf("drive forward for %d seconds\n", tijd);
			sleep(tijd);
			//controlleerpositie(tijd);
			break;
		//3 achteruit rijden
		case 3:
			warning=call->move(-LSPEED,RSPEED);
			warning=call->move(-LSPEED,RSPEED);
			printf("drive backward for %d seconds\n", tijd);
			sleep(tijd);
			//controlleerpositie(tijd);
			break;
		default:
			warning=call->move(0,0);
			warning=call->move(0,0);
			printf("stop\n");
			break;
	}
	warning=call->move(0,0);
	warning=call->move(0,0);
	printf("stop\n");
	printf("stop\n");
}
void sendcmd::controlleerdraai(int tijd)
{
	int tmptijd=0;
	bool warning;
	do
	{
		WifiClient->receive(robot,nutteloos,obstakels);
		if(robotd_angle==robot->get_angle())
		{
			warning=call->move(0,0);
			warning=call->move(0,0);
		}
		else
			sleep(1);
		tmptijd++;
	}while(tmptijd<tijd);
	warning=call->move(0,0);
	warning=call->move(0,0);
}
void sendcmd::controlleerpositie(int tijd)
{
	int tmptijd=0;
	bool warning;
	double euclideanvorig, euclideannu;

	
	euclideanvorig=((robotd_x-robot->get_robotX())*(robotd_x-robot->get_robotX()))+((robotd_y-robot->get_robotY())*(robotd_y-robot->get_robotY()));
	do
	{	
		WifiClient->receive(robot,nutteloos,obstakels);
		euclideannu=((robotd_x-robot->get_robotX())*(robotd_x-robot->get_robotX()))+((robotd_y-robot->get_robotY())*(robotd_y-robot->get_robotY()));
		if(euclideannu>euclideanvorig)
		{
			warning=call->move(0,0);			
			warning=call->move(0,0);
		}
		else
			sleep(1);
		tmptijd++;
		euclideanvorig=euclideannu;
	}while(tmptijd<tijd);
	warning=call->move(0,0);
	warning=call->move(0,0);
}
bool sendcmd::controleernabereikt()
{
	if(robotd_x==robot->get_robotX() && robotd_y==robot->get_robotY())
		return true;
	else
	{
		int angle = cmdgen->calculateTurn(robot->get_angle(),robotd_angle);
		Point nu,volgende;

		nu.setX(robot->get_robotX());
		nu.setY(robot->get_robotY());
		volgende.setX(robotd_x);
		volgende.setY(robotd_y);
	
		int afstand=cmdgen->calculateDistance(nu,volgende);
		drive(afstand,angle);
	}
}
void sendcmd::closegrab()
{
	call->grab();
}
void sendcmd::opengrab()
{
	call->release();
}
